

/**
 *
 * create by shjzhang, 2020/03/02
 *
 * @file CSDetectorWL.cpp
 *
 */

#include "CSDetectorWL.hpp"
#include "SatID.hpp"
#include "constants.hpp"

#define debug 1

namespace gpstk
{

      // Returns a string identifying this object.
   std::string CSDetectorWL::getClassName() const
   { return "CSDetectorWL"; }
    

      /* Returns a satTypeValueMap object, adding the new data generated
       *  when calling this object.
       *
       * @param epoch     Time of observations.
       * @param gData     Data object holding the data.
       * @param epochflag Epoch flag.
       */
   satTypeValueMap& CSDetectorWL::Process( const CommonTime& epoch,
                                           satTypeValueMap& gData)
      noexcept(false)
   {

      try
      {
          if(debug)
          cout << "CSDetectorWL" << endl;

          if(debug)
          {
              cout << "current epoch" << endl;
              cout << "epoch:" << YDSTime(epoch) << endl;
              gData.dump(cout,1);

              cout << "last epoch" << endl;
              stvDataPrev.dump(cout,1);
          }

          // let's compute the time-difference data for cycle-slip detection
          getEpochDiffData(gData);

          // store the satTypeValueMap
          stvDataPrev = gData;

          DeltaX deltax;
          if(pLCDetector!=NULL)
          {
              deltax = pLCDetector->getDeltaX();
          }
          else
          {
              cerr << "pLCDetector is NULL" << endl;
              exit(-1);
          }

          if(firstTime)
          {
              firstTime = false;
              return gData;
          }


             // Loop through all the satellites
          for (auto& stv: gData )
          {
              SatID sat = stv.first;

              if(sat.system != satSys)
              {
                  continue;
              }

              double deltaPrefit;
              try
              {
                  // Try to extract the values
                  deltaPrefit = (stv).second(diffPrefitType);
              }
              catch(...)
              {
                  continue;
              }

              /// get dx/dy/dz/dcdt deriative from satTypeValueMap
              /// Notice:
              /// L(t) - L(t-1) = rou - drou(t)*dx;
              double DX, DY, DZ, DT;
              try
              {
                 DX = (stv).second[TypeID::dStaX];
                 DY = (stv).second[TypeID::dStaY];
                 DZ = (stv).second[TypeID::dStaZ];
                 DT = (stv).second[TypeID::dcdtSta];
              }
              catch(Exception& e)
              {
                 std::cerr 
                     << getClassName() 
                     << ": TypeID not found" << std::endl;
                 exit(-1);
              }

              double deltaRou = DX*deltax.dx + DY*deltax.dy + DZ*deltax.dz + DT*deltax.dt;

              if(debug)
              {
                  cout << "sat:" << sat << endl;
                  cout << "deltaPrefit:" << deltaPrefit << endl;
                  cout << "deltaRou:" << deltaRou << endl;
              }

              double deltaWL = deltaPrefit - deltaRou;

              cout << "deltaWL" << deltaWL << endl;

              // if the sampling rate is low, e.g. 30s and under active
              // ionopsheric event, you'd better fit and predict deltaIono
              // to substract the deltaIono from deltaWL and then the cycle
              // slip will be detected more precisely.
              cout << "usePolyFit:" << usePolyFit << endl;

              if(usePolyFit)
              {
                  int csFlag(0);
                  int csValue(0);
                  getDetection(sat, epoch, deltaWL, csFlag, csValue);

                  if(debug)
                  {
                      cout << "csFlag:" << csFlag << endl;
                      cout << "csValue:" << csValue << endl;
                  }

                  if(csFlag == 9)
                  {
                      stv.second[TypeID::CSWL] = 9;
                  }
                  else if(csFlag == 1)
                  {
                      stv.second[TypeID::CSWL] = 1;

                      // now, let's compute the ionospheric delays using prefitWL,
                      // convert it to the value on L1
                      double diffIono = -L2_FREQ_GPS/L1_FREQ_GPS*(deltaWL - waveLength*csValue);
                      stv.second[diffIonoType] = diffIono;
                  }
                  else if(csFlag == 0)
                  {
                      stv.second[TypeID::CSWL] = 0;

                      // now, let's compute the ionospheric delays using prefitWL,
                      // convert it to the value on L1
                      double diffIono = -L2_FREQ_GPS/L1_FREQ_GPS*(deltaWL - waveLength*csValue);
                      stv.second[diffIonoType] = diffIono;
                  }

              }
              // if the sampling rate is very high, e.g. 10s, 5s or 1s interval 
              // the deltaIono will be about zero, then detect the 
              // cycle slips directly.
              else 
              {
                  int csValue(0);
                  int csFlag(0);
                  if( std::abs(deltaWL) > (threshold - ionoDrift*sampleInterval) )
                  {
                      csValue = std::floor(deltaWL+0.5);

                      // cycle-slip happened
                      stv.second[TypeID::CSWL] = 1;

                      // now, let's compute the ionospheric delays using prefitWL,
                      // convert it to the value on L1
                      double diffIono = -L2_FREQ_GPS/L1_FREQ_GPS*(deltaWL - waveLength*csValue);
                      stv.second[diffIonoType] = diffIono;

                  }
                  else
                  {
                      // cycle-slip happened
                      stv.second[TypeID::CSWL] = 0;

                      // now, let's compute the ionospheric delays using prefitWL,
                      // convert it to the value on L1
                      double diffIono = -L2_FREQ_GPS/L1_FREQ_GPS*(deltaWL - waveLength*csValue);
                      stv.second[diffIonoType] = diffIono;
                  }
              }
          }

          // copy
          satTypeValueMap tempSatData = gData;

          TypeIDSet typeSet;

          typeSet.insert(TypeID::prefitL12);
          typeSet.insert(TypeID::prefitL15);
          typeSet.insert(TypeID::prefitL17);
          typeSet.insert(TypeID::prefitL18);
          typeSet.insert(TypeID::prefitL16);
          typeSet.insert(TypeID::prefitL27);
          typeSet.insert(TypeID::prefitL26);

          typeSet.insert(diffPrefitType);
          typeSet.insert(diffPostType);

          typeSet.insert(TypeID::CSLC);
          typeSet.insert(TypeID::CSWL);

          typeSet.insert(TypeID::interrupt);

          tempSatData.keepOnlyTypeID(typeSet);

          if(pCSDataStream!=NULL)
          {
              for(auto stv: tempSatData)
              {
                  (*pCSDataStream) 
                      << YDSTime(epoch) << " "
                      << stv.first << " "
                      << stv.second << " "
                      << endl;
              }
          }

          return gData;

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

   }  // End of method 'CSDetectorWL::Process()'


   int CSDetectorWL::getDetection(SatID& sat,
                                  const CommonTime& epoch, 
                                  double& deltaWL, 
                                  int& csFlag,
                                  int& csValue)
   {
       double currentDeltaT(0.0);

       int size( deltaData[sat].epochDeque.size() );

       // Get the difference between current epoch and LAST epoch,
       // in seconds, but first test if we have epoch data inside LIData
       // data break 
       if(size > 0)
       {
           currentDeltaT = ( epoch - deltaData[sat].epochDeque.back() );
       }
       // first epoch
       else
       {
           csFlag = 9;
           csValue = 0.0;

           // just insert the data into deltaData
           deltaData[sat].epochDeque.push_back(epoch);
           deltaData[sat].deltaDeque.push_back(deltaWL);

           return 0;
       }

       if(currentDeltaT>deltaTMax)
       {
           deltaData[sat].epochDeque.clear();
           deltaData[sat].deltaDeque.clear();
       }

       size = deltaData[sat].epochDeque.size();

       cout << "size:" << size << endl;

       if(size >= minBufferSize)
       {
           // Declare a Vector for deltaWL measurements
           Vector<double> y(size, 0.0);

           // Declare a Matrix for epoch information
           Matrix<double> M(size, polyOrder+1, 0.0);

           // weight matrix
           Matrix<double> wMatrix(size, size, 0.0);

              // We store here the OLDEST (or FIRST) epoch in buffer for future
              // reference. This is important because adjustment will be made
              // with respect to that first epoch
           CommonTime firstEpoch (deltaData[sat].epochDeque.front());

              // Feed 'y' with data
           for(size_t i=0; i<size; i++)
           {
                 // The newest goes first in 'y' vector
              y(i) = deltaData[sat].deltaDeque[i];
           }

           if(debug)
           {
              cout << "y" << endl;
              cout << y << endl;
           }

              // Feed 'M' with data
           for(size_t i=0; i<size; i++)
           {
                  // Compute epoch difference with respect to FIRST epoch
               double dT( deltaData[sat].epochDeque[i] - firstEpoch );

               if(polyOrder==1)
               {
                   M(i,0) = 1.0;
                   M(i,1) = dT;
               }
               if(polyOrder==2)
               {
                   M(i,0) = 1.0;
                   M(i,1) = dT;
                   M(i,2) = dT*dT;
               }
           }

           if(debug)
           {
               cout << "M" << endl;
               cout <<  M  << endl;
           }

           // Unit matrix
           for(size_t i=0; i<size; i++ )
           {
              wMatrix(i,i) = 1.0;
           }

           Vector<double> postfitResiduals;
           Vector<double> standResidual;
           double sigma;
           Vector<int> outlierVector;
           Vector<double> solution;
           Matrix<double> covMatrix;

           // get robust solution using current data
           Robust( y,
                   M,
                   wMatrix,
                   postfitResiduals,
                   standResidual,
                   solution,
                   covMatrix,
                   sigma,
                   outlierVector);

           if(debug)
           {
               cout << "sigma:" << sigma << endl;
               cout << "postfitResiduals:" << postfitResiduals << endl;
               cout << "standResidual:" << standResidual << endl;
               cout << "solution:" << solution << endl;
           }

           // let's judge whether the modelling of deltaWL is successful.
           // at the beginning of each new arc, the cycle slips will happened
           // frequently, which will cause failure of the fitting. 
           if( sigma > (0.86/4.0) )
           {
               csFlag = 9;

               // if modelling failed, just insert new data into deque, 
               // we will do the window-fitting next epoch
               deltaData[sat].epochDeque.push_back(epoch);
               deltaData[sat].deltaDeque.push_back(deltaWL);

           }
           else
           {
                  // Compute epoch difference with respect to FIRST epoch
               cout << "firstEpoch:"<< firstEpoch << endl;
               cout << "epoch:"<< epoch << endl;
               double deltaT( epoch - firstEpoch );

                  // Compute current adjusted delta value
               double predictWL( solution(0) + solution(1)*deltaT + solution(2)*deltaT*deltaT );

               // Difference between current and adjusted WL values
               double currentBias( std::abs( deltaWL - predictWL ) );

               if(debug)
               cout << "currentBias:" << currentBias <<endl;

               if( currentBias > (waveLength/2.0) )
               {
                   csFlag = 1.0;
                   csValue = std::floor( currentBias/waveLength +0.5 );
               }  
               // if no cycle slip happened, insert data into deque
               else
               {
                   csFlag = 0.0;
                   csValue = 0.0;

                   deltaData[sat].epochDeque.push_back(epoch);
                   deltaData[sat].deltaDeque.push_back(deltaWL);
               }
           } 
       }
       // if don't have enough data to start window-fitting
       else
       {
           csFlag = 9;
           csValue = 0.0;

           // just insert the data into deltaData
           deltaData[sat].epochDeque.push_back(epoch);
           deltaData[sat].deltaDeque.push_back(deltaWL);
       }

                // Update current buffer size
       size = deltaData[sat].epochDeque.size();

          // Check if we have exceeded maximum window size
       if(size > maxBufferSize)
       {
             // Get rid of oldest data, which is at the beginning of deque
          deltaData[sat].epochDeque.pop_front();
          deltaData[sat].deltaDeque.pop_front();
       }

   };
   

   void CSDetectorWL::getEpochDiffData(satTypeValueMap& sData)
   {
          // loop the satellite
       for(auto& stv: sData)
       {
           SatID sat = stv.first;
           if(sat.system != satSys)
           {
               continue;
           }

           if(stvDataPrev.find(sat) != stvDataPrev.end())
           {
               typeValueMap tvDataPrev = stvDataPrev[sat];

               bool hasCurrPrefit(false);
               if( stv.second.find(prefitType) != stv.second.end() )
               {
                   hasCurrPrefit = true;;
               }

               bool hasPrevPrefit(false);
               if( tvDataPrev.find(prefitType) != tvDataPrev.end() )
               {
                   hasPrevPrefit = true;;
               }

               double currPrefit, prevPrefit;
               if(hasCurrPrefit && hasPrevPrefit)
               {
                   currPrefit = stv.second(prefitType);
                   prevPrefit = tvDataPrev(prefitType);
                   stv.second[diffPrefitType] = currPrefit - prevPrefit;
               }
               else if( hasCurrPrefit && !hasPrevPrefit )
               {
                   // data break happened
                   stv.second[TypeID::CSLC] = 9.0;
               }
               else
               {
                   continue;
               }
           }
           else
           {
               stv.second[TypeID::CSWL] = 9.0;
           }
       }
   }

      /* Method to set the maximum interval of time allowed between two
       * successive epochs.
       *
       * @param maxDelta      Maximum interval of time, in seconds
       */
   CSDetectorWL& CSDetectorWL::setDeltaTMax(const double& maxDelta)
   {
         // Don't allow delta times less than or equal to 0
      if (maxDelta > 0.0)
      {
         deltaTMax = maxDelta;
      }
      else
      {
         deltaTMax = 31.0;
      }

      return (*this);

   }  // End of method 'CSDetectorWL::setDeltaTMax()'


      /* Returns a gnnsRinex object, adding the new data generated when
       * calling this object.
       *
       * @param gData    Data object holding the data.
       */
   void CSDetectorWL::Process(gnssRinex& gData)
      noexcept(false)
   {

      try
      {
         Process( gData.header.epoch, gData.body);

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

   }  // End of method 'CSDetectorWL::Process()'

      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int CSDetectorWL::Robust( Vector<double>& prefitResiduals,
                             Matrix<double>& designMatrix,
                             Matrix<double>& weightMatrix,
                             Vector<double>& postfitResiduals,
                             Vector<double>& standResidual,
                             Vector<double>& solution,
                             Matrix<double>& covMatrix,
                             double& sig,
                             Vector<int>& outlierVector)
       noexcept(false)
   {

         // By default, results are invalid
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("Compute(): Weight matrix is not square");
         cerr << e << endl;
         exit(-1);
      }

      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match!");
         cerr << e << endl;
         exit(-1);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("Compute(): size does not match !");
         cerr << e << endl;
         exit(-1);
      }

      int gCol = static_cast<int>(designMatrix.cols());

         // After checking sizes, let's do the real correction work
      Matrix<double> AT( transpose(designMatrix) );

      int numEqu( prefitResiduals.size() );
      int numUnknowns( designMatrix.cols() );

      Matrix<double> tempWeight = weightMatrix;

      Vector<double> stdResidual(pRow, 0.0);
      Vector<double> robustResidual(pRow, 0.0);

      cout << "prefitResiduals" << endl;
      cout <<  prefitResiduals  << endl;

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      cout << "postfitResiduals" << endl;
      cout <<  postfitResiduals  << endl;

      double VTPV;
      VTPV = 0.0;
      for( int i = 0; i < pRow; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
          cout <<"v:" << postfitResiduals(i) << ": weight:" << tempWeight(i,i) << endl;
          cout << "vtpv:" << postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i) << endl;
      }

      // update the weight
      // warning, the numvalid should be valid number
      double sigma;
      sigma = std::sqrt( VTPV /(numEqu - numUnknowns) );

      cout <<"sigma" << endl;
      cout << sigma << endl;


      // Covariance for observation;
      Matrix<double> Qvv(pRow, pRow, 0.0);

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<numEqu; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }
  
      Vector<int> goodVec(numEqu, 0);
      int numValid = 0;
      for(int i=0; i<numEqu; i++)
      {
          int indexMin = getMinIndex(stdResidual, goodVec);

          // we need at least numUnknowns satellite to compute the difference of x and t
          if( stdResidual(i) < 1.5 )
          {
              goodVec(indexMin) = 1;
              numValid++;
          }
      }

      Vector<int> outlierVec(numEqu, 0);;
      if(numValid < numUnknowns)
      {
          for(int i=0; i<numEqu; i++)
          {
              outlierVec(i) = 1;
              sigma = 1.0E+8;  // a very big value;
          }

          return 0;
      }

      cout << "goodVec" << endl;
      cout << prefitResiduals << endl;
      cout << postfitResiduals << endl;
      cout << stdResidual << endl;
      cout << goodVec << endl;

      tempWeight = weightMatrix;
      for(int i=0; i<numEqu; i++)
      {
          if(goodVec(i) == 0)
          {
              tempWeight(i,i) = 1.0E-8;
          }
      }

      cout <<"tempWeight" << endl;
      cout << tempWeight << endl;

      // robust estimation

      /////////////////////
      // Solve the robust x with robust observation
      /////////////////////

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
         solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      cout << "postfitResiduals-1" << endl;
      cout <<  postfitResiduals  << endl;

      VTPV = 0.0;
      for( int i = 0; i < numEqu; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      sigma = std::sqrt( VTPV /(numValid - numUnknowns) );

      cout <<"sigma" << endl;
      cout << sigma << endl;

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;
  
      cout << "Qvv" << endl;
      cout << Qvv << endl;

      Qvv = inverseChol(weightMatrix);

      cout << "Qvv" << endl;
      cout << Qvv << endl;

      for(int i=0; i<numEqu; i++)
      {
          cout << postfitResiduals(i) << endl;
          cout << sigma << endl;
          cout << std::sqrt(Qvv(i,i)) << endl;
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      cout << "stdResidual-1" << endl;
      cout <<  stdResidual << endl;

      //////////////
      // at last, decide the robust outlier using robust stdResidual
      // and solve the equation using the clean observations
      //////////////

      tempWeight = weightMatrix;
      numValid = numEqu;

      outlierVec.resize(numEqu, 0);
      for(int i=0; i<pRow; i++)
      {
          if( stdResidual(i) > 3.5 )
          {
              outlierVec(i) = 1;
              tempWeight(i,i) = 1.0E-8; 
          }
      }

      try
      {
         Matrix<double> invTemp;
         invTemp = ( AT * tempWeight* designMatrix );
         covMatrix = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
         solution = covMatrix * ( (AT * tempWeight * prefitResiduals) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);

      cout << "postfitResiduals-2" << endl;
      cout <<  postfitResiduals  << endl;

      VTPV = 0.0;
      for( int i = 0; i < numEqu; ++i)
      {
          VTPV += postfitResiduals(i)*postfitResiduals(i)*tempWeight(i,i);
      }

      sigma = std::sqrt( VTPV /(numValid - numUnknowns) );

      cout <<"sigma" << endl;
      cout << sigma << endl;

      // warning: here the weightMatrix should be given for all observations
      Qvv = inverseChol(weightMatrix) - designMatrix * covMatrix * AT;

      for(int i=0; i<pRow; i++)
      {
          stdResidual[i] =  std::abs( postfitResiduals(i) /(sigma*std::sqrt(Qvv(i,i))) ) ;
      }

      cout << "stdResidual-2" << endl;
      cout <<  stdResidual << endl;


      ///////////////////
      // return
      ///////////////////

      standResidual = stdResidual;
      outlierVector = outlierVec;
      sig = sigma;

      return 0;

   }  // End of method 'CSDetectorWL::MeasUpdate()'


   int CSDetectorWL::getMaxIndex(Vector<double>& residual, 
                                 Vector<int>& flagVec)
   {
       double max=0.0;
       int indexMax = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i))>max && flagVec(i) !=1 )
           {
               max = std::abs(residual(i));
               indexMax = i;
           }
       }
       return indexMax;
   };


   int CSDetectorWL::getMinIndex(Vector<double>& residual, 
                                 Vector<int>& flagVec)
   {
       double min= 1.0E+8;
       int minIndex = 0;
       for(int i=0; i<residual.size(); i++)
       {
           if(std::abs(residual(i)) < min && flagVec(i) !=1 )
           {
               min = std::abs(residual(i));
               minIndex = i;
           }
       }
       return minIndex;
   };



}  // End of namespace gpstk
